#pragma once
#include<iostream>
#include<vector>
#include<Eigen\Dense>
#include<Eigen\Sparse>

using namespace Eigen;
using namespace std;
const double epsilon = 1e-7;

class Function
{
public:
    virtual double operator () (const double& x) const = 0;
    virtual double derivative(const double& x) const
    {
        return ((*this)(x + epsilon) - (*this)(x - epsilon)) / (epsilon * 2);
    }
    virtual double sec_derivative(const double& x) const
    {
        return ((*this).derivative(x + epsilon) - (*this).derivative(x - epsilon)) / (epsilon * 2);
    }
};

class Polynomial : public Function
{
private:
    vector<double> coefficient;
    double x_0;
public:
    Polynomial(const Polynomial& p) :coefficient(p.coefficient), x_0(p.x_0) {}
    Polynomial(const vector<double>& coe, const double x) : coefficient(coe), x_0(x) {}

    double operator() (const double& x) const
    {
        int n = coefficient.size();
        double re = 0, x_1 = x - x_0;
        for (int i = n - 1;i >= 0;i--) re = coefficient[i] + re * x_1;
        return re;
    }
    double root() { return x_0; }
};

class ppForm_spline
{
private:
    vector<double> t;
    vector<Polynomial> P;
public:
    ppForm_spline() {}
    ppForm_spline(const vector<double>& t_0, const vector<Polynomial>& P_0) { t = t_0;P = P_0; }
    void update(vector<double> t_0, vector<Polynomial> P_0) { t = t_0;P = P_0; }

    double operator () (const double& x) const
    {
        if (t.size() == 0) cerr << "error: Not uninitialized!" << endl, exit(-1);
        if (x<t.front() || x>t.back())cerr << "error: Exceeding the value range!" << endl, exit(-1);
        for (int i = 0;i < t.size() - 1;i++) if (x <= t[i + 1]) return P[i](x);
        return P[t.size() - 1](x);
    }
};

class ppForm_interpolation
{
private:
    ppForm_spline P;
    Function& f;
    int n, condition, order;
    vector<double> x;
public:
    ppForm_interpolation(Function& f, vector<double>& x, int order, int condi) :f(f), x(x), order(order), condition(condi)
    {
        if (!(condition >= 1 && condition <= 3 || order == 1)) cerr << "error: Unavailable condition!" << endl, exit(-1);
        if (!(order == 1 || order == 3)) cerr << "error: Unavailable order!" << endl, exit(-1);
        n = x.size();
    }
    void solve()
    {
        if (order == 3)
        {
            SparseMatrix<double> A(n, n);
            vector<Triplet<double>> a;
            MatrixXd b(n, 1);
            b = MatrixXd::Zero(n, 1);
            for (int i = 1;i < n - 1;i++)
            {
                double lamda = (x[i + 1] - x[i]) / (x[i + 1] - x[i - 1]);
                double miu = (x[i] - x[i - 1]) / (x[i + 1] - x[i - 1]);
                a.push_back(Triplet<double>(i, i - 1, lamda));
                a.push_back(Triplet<double>(i, i, 2));
                a.push_back(Triplet<double>(i, i + 1, miu));
                b(i, 0) = 3 * miu * (f(x[i + 1]) - f(x[i])) / (x[i + 1] - x[i]) + 3 * lamda * (f(x[i]) - f(x[i - 1])) / (x[i] - x[i - 1]);
            }
            if (condition == 1)
            {
                a.push_back(Triplet<double>(0, 0, 1));
                a.push_back(Triplet<double>(n - 1, n - 1, 1));
                b(0, 0) = f.derivative(x[0]);
                b(n - 1, 0) = f.derivative(x[n - 1]);
            }
            else if (condition == 2)
            {
                a.push_back(Triplet<double>(0, 0, 4));
                a.push_back(Triplet<double>(0, 1, 2));
                a.push_back(Triplet<double>(n - 1, n - 2, 2));
                a.push_back(Triplet<double>(n - 1, n - 1, 4));
                b(0, 0) = 6 * (f(x[1]) - f(x[0])) / (x[1] - x[0]) - f.sec_derivative(x[0]) * (x[1] - x[0]);
                b(n - 1, 0) = 6 * (f(x[n - 1]) - f(x[n - 2])) / (x[n - 1] - x[n - 2]) - f.sec_derivative(x[n - 1]) * (x[n - 1] - x[n - 2]);
            }
            else
            {
                a.push_back(Triplet<double>(0, 0, 4));
                a.push_back(Triplet<double>(0, 1, 2));
                a.push_back(Triplet<double>(n - 1, n - 2, 2));
                a.push_back(Triplet<double>(n - 1, n - 1, 4));
                b(0, 0) = 6 * (f(x[1]) - f(x[0])) / (x[1] - x[0]);
                b(n - 1, 0) = 6 * (f(x[n - 1]) - f(x[n - 2])) / (x[n - 1] - x[n - 2]);
            }

            A.setFromTriplets(a.begin(), a.end());
            A.makeCompressed();
            SparseLU<SparseMatrix<double>> Solver;
            Solver.compute(A);
            VectorXd m = Solver.solve(b);

            vector<Polynomial> S;
            for (int i = 1;i < n;i++)
            {
                vector<double> coe;
                double K = (f(x[i]) - f(x[i - 1])) / (x[i] - x[i - 1]);
                coe.push_back(f(x[i - 1]));
                coe.push_back(m(i - 1));
                coe.push_back((K * 3 - m(i - 1) * 2 - m(i)) / (x[i] - x[i - 1]));
                coe.push_back((m(i - 1) + m(i) - K * 2) / pow(x[i] - x[i - 1], 2));
                Polynomial p(coe, x[i - 1]);
                S.push_back(p);
            }
            P.update(x, S);
        }
        else
        {
            vector<double> t(x);
            vector<Polynomial> S;
            int n = x.size();
            for (int i = 1;i < n;i++)
            {
                vector<double> coe;
                double K = (f(x[i]) - f(x[i - 1])) / (x[i] - x[i - 1]);
                coe.push_back(f(x[i]) - K * x[i]);
                coe.push_back(K);
                Polynomial p(coe, 0);
                S.push_back(p);
            }
            P.update(t, S);
        }
    }
    double operator () (const double& x)const
    {
        return P(x);
    }
};

class B_spline : public Function
{
private:
    vector<double> t;
    int k;
public:
    B_spline(vector<double> t, int k) : t(t), k(k)
    {
        if (k != t.size() - 2) cerr << "error: Polynomial degree does not match the number of coefficients!" << endl, exit(-1);
    }
    void update(vector<double> t_0, int k_0) { t = t_0;k = k_0; }

    double operator() (const double& x) const
    {
        if (k == 1)
        {
            if (x < t[1]) return max(0.0, (x - t[0]) / (t[1] - t[0]));
            else return max(0.0, (t[2] - x) / (t[2] - t[1]));
        }
        vector<double> t1(t.begin(), t.end() - 1);
        vector<double> t2(t.begin() + 1, t.end());
        B_spline B1(t1, k - 1);
        B_spline B2(t2, k - 1);
        return (x - t[0]) * B1(x) / (t[k] - t[0]) + (t[k + 1] - x) * B2(x) / (t[k + 1] - t[1]);
    }
};

class B_spline_interpolation
{
private:
    vector<B_spline> B;
    vector<double> coe;
    Function& f;
    int n, condition, order;
    vector<double> x, front_point, end_point;
public:
    B_spline_interpolation(Function& f_0, vector<double>& x_0, int order_0, int condi_0) :f(f_0), x(x_0), order(order_0), condition(condi_0)
    {
        if (!(condition >= 1 && condition <= 3 || order == 1))cerr << "error: Unavailable condition!" << endl, exit(-1);
        if (!(order >= 1 && order <= 3))cerr << "error: Unavailable order!" << endl, exit(-1);
        n = x.size();

        double del = (x[1] - x[0]);
        if (order == 3)
        {
            front_point = { x[0] - del * 3,x[0] - del * 2,x[0] - del };
            end_point = { x[n - 1] + del,x[n - 1] + del * 2,x[n - 1] + del * 3 };
        }
        else if (order == 2)
        {
            front_point = { x[0] - del * 2,x[0] - del };
            end_point = { x[n - 1] + del,x[n - 1] + del * 2,x[n - 1] + del * 3 };
        }
        else
        {
            front_point = { x[0] - del };
            end_point = { x[n - 1] + del };
        }
        x.insert(x.begin(), front_point.begin(), front_point.end());
        x.insert(x.end(), end_point.begin(), end_point.end());
        if (order == 2)
        {
            for (int i = 0; i < n + 5; i++)
                x[i] -= 0.5;
        }
        for (int i = 0;i < n + order - 1;i++)
        {
            vector<double> X(x.begin() + i , x.begin() + i + order + 2);
            B_spline Bi(X, order);
            B.push_back(Bi);
        }
    }
    void solve()
    {
        if (order == 3)
        {
            SparseMatrix<double> A(n + 2, n + 2);
            vector<Triplet<double>> a;
            MatrixXd b(n + 2, 1);
            b = MatrixXd::Zero(n + 2, 1);
            for (int i = 1;i < n + 1;i++)
            {
                a.push_back(Triplet<double>(i, i - 1, B[i - 1](x[i + 2])));
                a.push_back(Triplet<double>(i, i, B[i](x[i + 2])));
                a.push_back(Triplet<double>(i, i + 1, B[i + 1](x[i + 2])));
                b(i, 0) = f(x[i + 2]);
            }

            if (condition == 1)
            {
                B_spline B0({ x[1],x[2],x[3],x[4] }, 2);
                B_spline B1({ x[2],x[3],x[4],x[5] }, 2);
                B_spline Bn_minus1(vector<double>(x.end() - 6, x.end() - 2), 2);
                B_spline Bn(vector<double>(x.end() - 5, x.end() - 1), 2);

                a.push_back(Triplet<double>(0, 0, -B0(x[3]) * 3 / (x[4] - x[1])));
                a.push_back(Triplet<double>(0, 1, B0(x[3]) * 3 / (x[4] - x[1]) - B1(x[3]) * 3 / (x[5] - x[2])));
                a.push_back(Triplet<double>(0, 2, B1(x[3]) * 3 / (x[5] - x[2])));

                a.push_back(Triplet<double>(n + 1, n - 1, -Bn_minus1(x[n + 2]) * 3 / (x[n + 3] - x[n])));
                a.push_back(Triplet<double>(n + 1, n, Bn_minus1(x[n + 2]) * 3 / (x[n + 3] - x[n]) - Bn(x[n + 2]) * 3 / (x[n + 4] - x[n + 1])));
                a.push_back(Triplet<double>(n + 1, n + 1, Bn(x[n + 2]) * 3 / (x[n + 4] - x[n + 1])));

                b(0, 0) = f.derivative(x[3]);
                b(n + 1, 0) = f.derivative(x[n + 2]);
            }
            else if (condition == 2)
            {
                a.push_back(Triplet<double>(0, 0, (x[4] - x[2]) / (x[4] - x[1]) * 6));
                a.push_back(Triplet<double>(0, 1, -(x[4] - x[2]) / (x[4] - x[1]) * 6 - (x[4] - x[2]) / (x[5] - x[2]) * 6));
                a.push_back(Triplet<double>(0, 2, (x[4] - x[2]) / (x[5] - x[2]) * 6));

                a.push_back(Triplet<double>(n + 1, n - 1, (x[n + 3] - x[n + 1]) / (x[n + 3] - x[n]) * 6));
                a.push_back(Triplet<double>(n + 1, n, -(x[n + 3] - x[n + 1]) / (x[n + 3] - x[n]) * 6 - (x[n + 3] - x[n + 1]) / (x[n + 4] - x[n + 1]) * 6));
                a.push_back(Triplet<double>(n + 1, n + 1, (x[n + 3] - x[n + 1]) / (x[n + 4] - x[n + 1]) * 6));
                b(0, 0) = f.sec_derivative(x[3]), b(n + 1, 0) = f.sec_derivative(x[n + 2]);
            }
            else
            {
                a.push_back(Triplet<double>(0, 0, (x[4] - x[2]) / (x[4] - x[1]) * 6));
                a.push_back(Triplet<double>(0, 1, -(x[4] - x[2]) / (x[4] - x[1]) * 6 - (x[4] - x[2]) / (x[5] - x[2]) * 6));
                a.push_back(Triplet<double>(0, 2, (x[4] - x[2]) / (x[5] - x[2]) * 6));

                a.push_back(Triplet<double>(n + 1, n - 1, (x[n + 3] - x[n + 1]) / (x[n + 3] - x[n]) * 6));
                a.push_back(Triplet<double>(n + 1, n, -(x[n + 3] - x[n + 1]) / (x[n + 3] - x[n]) * 6 - (x[n + 3] - x[n + 1]) / (x[n + 4] - x[n + 1]) * 6));
                a.push_back(Triplet<double>(n + 1, n + 1, (x[n + 3] - x[n + 1]) / (x[n + 4] - x[n + 1]) * 6));
            }
            A.setFromTriplets(a.begin(), a.end());
            A.makeCompressed();
            SparseLU<SparseMatrix<double> > Solver;
            Solver.compute(A);
            VectorXd X = Solver.solve(b);
            coe.resize(n + 2);
            for (int i = 0;i < n + 2;i++) coe[i] = X(i, 0);
        }
        else if (order == 2)
        {   
            SparseMatrix<double> A(n + 1, n + 1);
            vector<Triplet<double>> a;
            MatrixXd b(n + 1, 1);
            b = MatrixXd::Zero(n + 1, 1);
            for (int i = 1;i < n;i++)
            {
                a.push_back(Triplet<double>(i, i - 1, 1));
                a.push_back(Triplet<double>(i, i, 6));
                a.push_back(Triplet<double>(i, i + 1, 1));
                b(i, 0) = 8*f(x[i + 1]+0.5);
            }
            a.push_back(Triplet<double>(0, 0, 1));
            a.push_back(Triplet<double>(0, 1, 1));
            a.push_back(Triplet<double>(n, n - 1, 1));
            a.push_back(Triplet<double>(n, n, 1));
            b(0, 0) = 2 * f(x[1]);
            b(n, 0) = 2 * f(x[n + 1]);
            A.setFromTriplets(a.begin(), a.end());
            A.makeCompressed();
            SparseLU<SparseMatrix<double> > Solver;
            Solver.compute(A);
            VectorXd X = Solver.solve(b);
            coe.resize(n + 1);
            for (int i = 0;i < n + 1;i++) coe[i] = X(i, 0);
        }
        else
        {
            for (int i = 0;i < n;i++) coe.push_back(f(x[i + 1]));
        }
    }
    double operator () (const double& x) const
    {
        double re = 0;
        for (int i = 0;i < coe.size();i++)re += coe[i] * B[i](x);
        return re;
    }
};

class Discrete_function : public Function
{
private:
    vector<double> x, f, fd, fd2;
    double findvalue(const double& x_0, const vector<double> temp) const
    {
        for (int i = 0;i < x.size();i++)
            if (fabs(x[i] - x_0) < epsilon) return temp[i];
        cerr << "error: Without this point!" << endl, exit(-1);
    }
public:
    Discrete_function(vector<double>& x, vector<double>& f) :x(x), f(f) {}
    Discrete_function(vector<double>& x, vector<double>& f, vector<double>& fd) :x(x), f(f), fd(fd) {}
    Discrete_function(vector<double>& x, vector<double>& f, vector<double>& fd, vector<double>& fd2) :x(x), f(f), fd(fd), fd2(fd2) {}

    double operator()(const double& x_0) const
    {
        return findvalue(x_0, f);
    }
    double derivative(const double& x_0) const
    {
        return findvalue(x_0, fd);
    }
    double sec_derivative(const double& x_0) const
    {
        return findvalue(x_0, fd2);
    }
};

class curve_spline
{
private:
    vector<vector<double>> data, result;
    int order, condition, type, k, n;
    vector<double> length;
public:
    curve_spline(std::vector<vector<double>> data_0, int order_0, int con_0, int ty_0) :data(data_0), order(order_0), condition(con_0), type(ty_0)
    {
        k = data[0].size();
        n = data.size();
    }
    double dis(vector<double> x, vector<double> y)
    {
        double re = 0;
        if (x.size() != y.size())cerr << "error: Different dimention!" << endl, exit(-1);
        for (int i = 0;i < k;i++) re += pow((x[i] - y[i]),2);
        return sqrt(re);
    }
    void solve()
    {
        length.push_back(0);
        for (int i = 1;i < n;i++) length.push_back(length[i - 1] + dis(data[i], data[i - 1]));
        vector<Discrete_function> F;
        double del = length[n - 1] / n;
        for (int i = 0;i < k;i++)
        {
            vector<double> f, fd(n, 0), fd2(n, 0);
            for (int j = 0;j < n;j++) f.push_back(data[j][i]);
            for (int j = 1;j < n - 1;j++) fd[j] = (f[j + 1] - f[j - 1]) / (length[j + 1] - length[j - 1]);
            for (int j = 1;j < n - 1;j++) fd2[j] = (fd[j + 1] - fd[j - 1]) / (length[j + 1] - length[j - 1]);

            F.push_back(Discrete_function(length, f, fd, fd2));
        }
        result.resize(n + 1);
        if (type == 0)
        {
            for (int i = 0;i < k;i++)
            { 
                B_spline_interpolation B(F[i], length, order, condition);
                B.solve();
                for (int j = 0;j < n + 1;j++) result[j].push_back(B(j * del));
            }
        }
        else
        {
            for (int i = 0;i < k;i++)
            {
                ppForm_interpolation p(F[i], length, order, condition);
                p.solve();
                for (int j = 0;j < n + 1;j++) result[j].push_back(p(j * del));
            }
        }
    }
    vector<vector<double>> get_result() { return result; }
    double operator() (const double& x) const
    {
        cerr << "error: Not supported operator!" << endl, exit(-1);
    }
};